#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>
#include "rs274ngc.hh"
#include "rs274ngc_return.hh"
#include "nml_intf/interp_return.hh"
#include "interp_internal.hh"
#include "rs274ngc_interp.hh"

#include "rtapi/rtapi.h"

//========================================================================
// Functions for control stuff (O-words)
//========================================================================

/*
  Given the root of a directory tree and a file name,
  find the path to the file, if any.
*/

int Interp::findFile(       // ARGUMENTS
    char* direct,           // the directory to start looking in
    char* target,           // the name of the file to find
    char* foundFileDirect)  // where to store the result
{
    FILE* file;
    DIR* aDir;
    struct dirent* aFile;
    char targetPath[PATH_MAX + 1];

    snprintf(targetPath, PATH_MAX, "%s/%s", direct, target);
    file = fopen(targetPath, "r");
    if (file)
    {
        strncpy(foundFileDirect, direct, PATH_MAX);
        fclose(file);
        return INTERP_OK;
    }
    aDir = opendir(direct);
    if (!aDir)
    {
        ERS(INTERP_ERR_FILE_NOT_OPENED, NCE_FILE_NOT_OPEN);
    }

    while ((aFile = readdir(aDir)))
    {
        if (aFile->d_type == DT_DIR && (0 != strcmp(aFile->d_name, "..")) && (0 != strcmp(aFile->d_name, ".")))
        {
            char path[PATH_MAX + 1];
            snprintf(path, PATH_MAX, "%s/%s", direct, aFile->d_name);
            if (INTERP_OK == findFile(path, target, foundFileDirect))
            {
                closedir(aDir);
                return INTERP_OK;
            }
        }
    }
    closedir(aDir);
    ERS(INTERP_ERR_FILE_NOT_OPENED, NCE_FILE_NOT_OPEN);
}

/*
 *  this now uses STL maps for offset access
 */
int Interp::control_save_offset(block_pointer block,    /* pointer to a block of RS274/NGC instructions */
                                setup_pointer settings) /* pointer to machine settings */
{
    static char name[] = "control_save_offset";
    offset_pointer op = NULL;

    logOword("Entered:%s for o_name:|%s|", name, block->o_name);

    if ((block->o_type == O_if || block->o_type == O_elif || block->o_type == O_else || block->o_type == O_endif || block->o_type == O_do ||
         block->o_type == O_while || block->o_type == O_endwhile))
    {
        if (control_find_curroword(block, settings, &op) == INTERP_OK)
        {
            ERS(INTERP_ERR_REDEFINITION,
                _("File:%s line:%d redefining if: o|%s| already defined in file:%s"),
                settings->filename,
                settings->sequence_number,
                block->o_name,
                op->filename);
        }
    }
    else
    {
        if (control_find_oword(block, settings, &op) == INTERP_OK)
        {
            ERS(INTERP_ERR_REDEFINITION,
                _("File:%s line:%d redefining if: o|%s| already defined in file:%s"),
                settings->filename,
                settings->sequence_number,
                block->o_name,
                op->filename);
        }
    }

    offset new_offset;
    new_offset.type = block->o_type;
    new_offset.offset = block->offset;
    new_offset.filename = strstore(settings->filename);
    new_offset.repeat_count = -1;
    // the sequence number has already been bumped, so save
    // the proper value
    new_offset.sequence_number = settings->sequence_number - 1;
    settings->offset_map.insert(std::make_pair(block->o_name, new_offset));
    // rtapi_print("new offset: o_name=%s, type=%d,  offset=%d,  fileName=%s,  seq_number=%d\n", block->o_name, new_offset.type,new_offset.offset, new_offset.filename, new_offset.sequence_number);

    // print the saved offset
    // for(auto it = settings->offset_map.begin(); it != settings->offset_map.end(); it = settings->offset_map.upper_bound(it->first)) {
    //     rtapi_print("key word of o_name: %s\n", it->first);
    //     auto range = settings->offset_map.equal_range(it->first);
    //     while(range.first != range.second) {
    //         rtapi_print("	type=%d, line_num=%d\n", range.first->second.type, range.first->second.sequence_number);
    //         ++range.first;
    //     }
    // }

    return INTERP_OK;
}

int Interp::control_find_oword(block_pointer block,     // pointer to block
                               setup_pointer settings,  // pointer to machine settings
                               offset_pointer* op)      // pointer to offset descriptor
{
    static char name[] = "control_find_oword";
    offset_map_iterator it;

    it = settings->offset_map.find(block->o_name);
    if (it != settings->offset_map.end())
    {
        *op = &it->second;
        return INTERP_OK;
    }
    else
    {
        logOword("%s: Unknown oword name: |%s|", name, block->o_name);
        ERS(INTERP_ERR_UNKNOWN_OWORD_NUMBER, NCE_UNKNOWN_OWORD_NUMBER);
    }
}

int Interp::control_find_ifhead(block_pointer block, setup_pointer settings, offset_pointer* op)
{
    static char name[] = "control_find_ifhead";
    auto range_iter = settings->offset_map.equal_range(block->o_name);
    if (range_iter.first != std::end(settings->offset_map))
    {
        for (auto iter = range_iter.first; iter != range_iter.second; ++iter)
        {
            if (iter->second.type == O_if)
            {
                *op = &iter->second;
                return INTERP_OK;
            }
        }
    }
    logOword("%s: Unknown oword name: |%s|", name, block->o_name);
    return INTERP_ERROR;
}

int Interp::control_find_curroword(block_pointer block, setup_pointer settings, offset_pointer* op)
{
    static char name[] = "control_find_curroword";
    auto range_iter = settings->offset_map.equal_range(block->o_name);
    if (range_iter.first != std::end(settings->offset_map))
    {
        for (auto iter = range_iter.first; iter != range_iter.second; ++iter)
        {
            if (iter->second.offset == block->offset && iter->second.sequence_number == settings->sequence_number - 1)
            {
                *op = &iter->second;
                return INTERP_OK;
            }
        }
    }
    logOword("%s: Unknown oword name: |%s|", name, block->o_name);
    return INTERP_ERROR;
}

// find the next case of if sentense
int Interp::control_find_ifnext(block_pointer block, setup_pointer settings, offset_pointer* op)
{
    static char name[] = "control_find_curroword";
    auto range_iter = settings->offset_map.equal_range(block->o_name);
    int next_line = settings->sequence_number - 1;
    if (range_iter.first != std::end(settings->offset_map))
    {
        for (auto iter = range_iter.first; iter != range_iter.second; ++iter)
        {
            if ((next_line == settings->sequence_number - 1 && iter->second.sequence_number > next_line) ||
                (iter->second.sequence_number > settings->sequence_number - 1 && iter->second.sequence_number < next_line))
            {
                next_line = iter->second.sequence_number;
                *op = &iter->second;
            }
        }
    }
    if (next_line == settings->sequence_number - 1)
    {
        logOword("%s: Unknown next of oword name: |%s|", name, block->o_name);
        return INTERP_ERROR;
    }
    else
    {
        return INTERP_OK;
    }
}

// find the endif case of if sentense
int Interp::control_find_ifend(block_pointer block, setup_pointer settings, offset_pointer* op)
{
    static char name[] = "control_find_ifend";
    auto range_iter = settings->offset_map.equal_range(block->o_name);
    if (range_iter.first != std::end(settings->offset_map))
    {
        for (auto iter = range_iter.first; iter != range_iter.second; ++iter)
        {
            if (iter->second.type == O_endif)
            {
                // rtapi_print("found the end for label in %d\n", settings->sequence_number);
                *op = &iter->second;
                return INTERP_OK;
            }
        }
    }
    logOword("%s: Unknown oword name: |%s|", name, block->o_name);
    return INTERP_ERROR;
}

int Interp::control_find_whileend(block_pointer block, setup_pointer settings, offset_pointer* op)
{
    static char name[] = "control_find_whileend";
    auto range_iter = settings->offset_map.equal_range(block->o_name);
    if (range_iter.first != std::end(settings->offset_map))
    {
        for (auto iter = range_iter.first; iter != range_iter.second; ++iter)
        {
            if (iter->second.type == O_endwhile)
            {
                // rtapi_print("found the end for label in %d\n", settings->sequence_number);
                *op = &iter->second;
                return INTERP_OK;
            }
        }
    }
    logOword("%s: Unknown oword name: |%s|", name, block->o_name);
    return INTERP_ERROR;
}

const char* o_ops[] = {
    "O_none",
    "O_sub",
    "O_endsub",
    "O_call",
    "O_do",
    "O_while",
    "O_if",
    "O_elif",
    "O_else",
    "O_endif",
    "O_break",
    "O_continue",
    "O_endwhile",
    "O_return",
    "O_repeat",
    "O_endrepeat",
    "O_continue_call",
    "O_pyreturn",
};

const char* call_statenames[] = {
    "CS_NORMAL",
    "CS_REEXEC_PROLOG",
    "CS_REEXEC_PYBODY",
    "CS_REEXEC_EPILOG",
    "CS_REEXEC_PYOSUB",
};

const char* call_typenames[] = {
    "CT_NGC_OWORD_SUB",
    "CT_PYTHON_OWORD_SUB",
    "CT_REMAP",
};

int Interp::execute_call(setup_pointer settings, context_pointer current_frame, int call_type)
{
    int status = INTERP_OK;
    int i;

    context_pointer previous_frame = &settings->sub_context[settings->call_level - 1];

    block_pointer eblock = &EXECUTING_BLOCK(*settings);

    logOword("execute_call %s type=%s state=%s cl=%d rl=%d",
             current_frame->subName,
             call_typenames[call_type],
             call_statenames[settings->call_state],
             settings->call_level,
             settings->remap_level);

    switch (call_type)
    {
    case CT_NGC_OWORD_SUB:
        // copy parameters from context
        // save old values of parameters
        // save current file position in context
        // if we were skipping, no longer
        if (settings->skipping_o)
        {
            logOword("case O_call -- no longer skipping to:|%s|", settings->skipping_o);
            settings->skipping_o = NULL;
        }
        for (i = 0; i < INTERP_SUB_PARAMS; i++)
        {
            previous_frame->saved_params[i] = settings->parameters[i + INTERP_FIRST_SUBROUTINE_PARAM];
            settings->parameters[i + INTERP_FIRST_SUBROUTINE_PARAM] = eblock->params[i];
        }

        // if the previous file was NULL, mark positon as -1 so as not to
        // reopen it on return.
        if (settings->file_pointer == NULL)
        {
            previous_frame->position = -1;
        }
        else
        {
            previous_frame->position = ftell(settings->file_pointer);
        }

        // save return location
        previous_frame->filename = strstore(settings->filename);
        previous_frame->sequence_number = settings->sequence_number;
        logOword("saving return location[cl=%d]: %s:%d offset=%ld",
                 settings->call_level - 1,
                 previous_frame->filename,
                 previous_frame->sequence_number,
                 previous_frame->position);

        if (FEATURE(OWORD_N_ARGS))
        {
            // let any  Oword sub know the number of parameters
            CHP(add_named_param("n_args", PA_READONLY));
            jaka_value tmpVal;
            tmpVal.scalarValue = (double)eblock->param_cnt;
            CHP(store_named_param(settings, "n_args", tmpVal, OVERRIDE_READONLY));
        }

        // transfer control
        if (control_back_to(eblock, settings) == INTERP_ERROR)
        {
            settings->call_level--;
            ERS(INTERP_ERR_OPEN_FILE_FAILED, NCE_UNABLE_TO_OPEN_FILE, eblock->o_name);
        }
        break;

        // M73 auto-restore is of dubious value in a Python subroutine

        // add_parameters will decorate kwargs as per argspec
        // if named local parameters specified
        // fall through

        //settings->sequence_number = previous_frame->sequence_number;
        // fall through

        // epilog is not supported on python body -  makes no sense

        // call the NGC remap procedure
    }
    return status;
}

// this is executed only for NGC subs, either normal ones or part of a remap
// subs whose name is a Py callable are handled inline in execute_call()
// since there is no corresponding O_return/O_endsub to execute.
int Interp::execute_return(setup_pointer settings, context_pointer current_frame, int call_type)
{
    int status = INTERP_OK;

    logOword("execute_return %s type=%s state=%s", current_frame->subName, call_typenames[call_type], call_statenames[settings->call_state]);

    block_pointer cblock = &CONTROLLING_BLOCK(*settings);
    block_pointer eblock = &EXECUTING_BLOCK(*settings);
    context_pointer previous_frame = &settings->sub_context[settings->call_level - 1];

    // if level is not zero, in a call
    // otherwise in a defn
    // if we were skipping, no longer
    if (settings->skipping_o && (eblock->o_type == O_endsub))
    {
        logOword("case O_%s -- no longer skipping to:|%s|", (eblock->o_type == O_endsub) ? "endsub" : "return", settings->skipping_o);
        settings->skipping_o = NULL;
    }

    switch (call_type)
    {
        // leave_context() is done by falling through into CT_NGC_OWORD_SUB code
    // fall through to normal NGC return handling
    case CT_NGC_OWORD_SUB:
        if (settings->call_level != 0)
        {
            // restore subroutine parameters.
            for (int i = 0; i < INTERP_SUB_PARAMS; i++) { settings->parameters[i + INTERP_FIRST_SUBROUTINE_PARAM] = previous_frame->saved_params[i]; }

            // file at this level was marked as closed, so dont reopen.
            if (previous_frame->position == -1)
            {
                settings->file_pointer = NULL;
                strcpy(settings->filename, "");
            }
            else
            {
                if (settings->file_pointer == NULL)
                {
                    ERS(INTERP_ERR_FILE_NOT_OPENED, NCE_FILE_NOT_OPEN);
                }
                //!!!KL must open the new file, if changed
                if (0 != strcmp(settings->filename, previous_frame->filename))
                {
                    fclose(settings->file_pointer);
                    settings->file_pointer = fopen(previous_frame->filename, "r");
                    if (settings->file_pointer == NULL)
                    {
                        ERS(INTERP_ERR_CANNOT_REOPEN_FILE, NCE_CANNOT_REOPEN_FILE, previous_frame->filename, strerror(errno));
                    }
                    strcpy(settings->filename, previous_frame->filename);
                }
                fseek(settings->file_pointer, previous_frame->position, SEEK_SET);
                settings->sequence_number = previous_frame->sequence_number;
                logOword("endsub/return: %s:%d pos=%ld", settings->filename, previous_frame->sequence_number, previous_frame->position);
            }
            // cleanups on return:
            CHP(leave_context(settings, true));

            // if this was a remap frame we're done
            if (current_frame->context_status & REMAP_FRAME)
            {
                CHP(remap_finished(-cblock->phase));
            }

            settings->sub_name = 0;
            if (previous_frame->subName)
            {
                settings->sub_name = previous_frame->subName;
            }
            else
            {
                settings->sub_name = NULL;
            }
        }
        else
        {  // call_level == 0
            // a definition
            if (eblock->o_type == O_endsub)
            {
                CHKS((settings->defining_sub != 1), INTERP_ERR_SYNTAX_FAILED, NCE_NOT_IN_SUBROUTINE_DEFN);
                // no longer skipping or defining
                if (settings->skipping_o)
                {
                    logOword("case O_endsub in defn -- no longer skipping to:|%s|", settings->skipping_o);
                    settings->skipping_o = NULL;
                }
                settings->defining_sub = 0;
                settings->sub_name = NULL;
            }
        }
    }
    return status;
}

//
// TESTME!!! MORE THOROUGHLY !!!KL
//
// In the past, calls had to be to predefined subs
//
// Now they don't. Do things in the following sequence:
// 1 -- if o_word is already defined, just go back to it, else
// 2 -- if there is a file with the name of the o_word,
//             open it and start skipping (as in 3, below)
// 3 -- skip to the o_word (will be an error if not found)
//
int Interp::control_back_to(block_pointer block,     // pointer to block
                            setup_pointer settings)  // pointer to machine settings
{
    static char name[] = "control_back_to";
    char newFileName[PATH_MAX + 1];
    FILE* newFP;
    offset_map_iterator it;
    offset_pointer op;

    logOword("Entered:%s %s", name, block->o_name);

    it = settings->offset_map.find(block->o_name);
    if (it != settings->offset_map.end())
    {
        op = &it->second;
        if ((settings->filename[0] != 0) & (settings->file_pointer == NULL))
        {
            ERS(INTERP_ERR_FILE_NOT_OPENED, NCE_FILE_NOT_OPEN);
        }
        if (0 != strcmp(settings->filename, op->filename))
        {
            // open the new file...
            newFP = fopen(op->filename, "r");
            // set the line number
            settings->sequence_number = 0;
            strncpy(settings->filename, op->filename, sizeof(settings->filename));
            if (settings->filename[sizeof(settings->filename) - 1] != '\0')
            {
                fclose(settings->file_pointer);
                logOword("filename too long: %s", op->filename);
                ERS(INTERP_ERR_OPEN_FILE_FAILED, NCE_UNABLE_TO_OPEN_FILE, op->filename);
            }

            if (newFP)
            {
                // close the old file...
                if (settings->file_pointer)  // only close if it was open
                    fclose(settings->file_pointer);
                settings->file_pointer = newFP;
            }
            else
            {
                logOword("Unable to open file: %s", settings->filename);
                ERS(INTERP_ERR_OPEN_FILE_FAILED, NCE_UNABLE_TO_OPEN_FILE, settings->filename);
            }
        }
        if (settings->file_pointer)
        {  // only seek if it was open
            fseek(settings->file_pointer, op->offset, SEEK_SET);
            // rtapi_print("fseek file: %s, offset: %d\n", op->filename, op->offset);
        }
        settings->sequence_number = op->sequence_number;
        return INTERP_OK;
    }
    newFP = find_ngc_file(settings, block->o_name, newFileName);

    if (newFP)
    {
        logOword("fopen: |%s| OK", newFileName);
        settings->sequence_number = 0;

        // close the old file...
        if (settings->file_pointer)
            fclose(settings->file_pointer);
        settings->file_pointer = newFP;
        strncpy(settings->filename, newFileName, sizeof(settings->filename));
        if (settings->filename[sizeof(settings->filename) - 1] != '\0')
        {
            logOword("new filename '%s' is too long (max len %zu)\n", newFileName, sizeof(settings->filename) - 1);
            settings->filename[sizeof(settings->filename) - 1] = '\0';  // oh well, truncate the filename
        }
    }
    else
    {
        char* dirname = getcwd(NULL, 0);
        logOword("fopen: |%s| failed CWD:|%s|", newFileName, dirname);
        free(dirname);
    }

    settings->skipping_o = block->o_name;       // start skipping
    settings->skipping_to_sub = block->o_name;  // start skipping
    settings->skipping_start = settings->sequence_number;
    return INTERP_OK;
}

int Interp::control_jump_to(block_pointer block, setup_pointer settings, offset_pointer op)
{
    // static char name[] = "control_jump_to";
    FILE* newFP;
    if ((settings->filename[0] != 0) & (settings->file_pointer == NULL))
    {
        ERS(INTERP_ERR_FILE_NOT_OPENED, NCE_FILE_NOT_OPEN);
    }
    if (0 != strcmp(settings->filename, op->filename))
    {
        // open the new file...
        newFP = fopen(op->filename, "r");
        // set the line number
        settings->sequence_number = 0;
        strncpy(settings->filename, op->filename, sizeof(settings->filename));
        if (settings->filename[sizeof(settings->filename) - 1] != '\0')
        {
            fclose(settings->file_pointer);
            logOword("filename too long: %s", op->filename);
            ERS(INTERP_ERR_OPEN_FILE_FAILED, NCE_UNABLE_TO_OPEN_FILE, op->filename);
        }

        if (newFP)
        {
            // close the old file...
            if (settings->file_pointer)  // only close if it was open
                fclose(settings->file_pointer);
            settings->file_pointer = newFP;
        }
        else
        {
            logOword("Unable to open file: %s", settings->filename);
            ERS(INTERP_ERR_OPEN_FILE_FAILED, NCE_UNABLE_TO_OPEN_FILE, settings->filename);
        }
    }
    if (settings->file_pointer)
    {  // only seek if it was open
        fseek(settings->file_pointer, op->offset, SEEK_SET);
        // rtapi_print("fseek file: %s, offset: %ld\n", op->filename, op->offset);
    }
    settings->sequence_number = op->sequence_number;
    return INTERP_OK;
}

// prepare a new call frame.
int Interp::enter_context(setup_pointer settings, block_pointer block)
{
    logOword("enter_context cl=%d->%d type=%s", settings->call_level, settings->call_level + 1, call_typenames[block->call_type]);

    settings->call_level++;
    if (settings->call_level >= INTERP_SUB_ROUTINE_LEVELS)
    {
        ERS(INTERP_ERR_TOO_MANY_SUBROUTINE_LEVELS, NCE_TOO_MANY_SUBROUTINE_LEVELS);
    }
    context_pointer frame = &settings->sub_context[settings->call_level];
    // mark frame for finishing remap
    frame->context_status = 0;
    frame->subName = block->o_name;
    frame->call_type = block->call_type;  // distinguish call frames: oword,python,remap

    // rtapi_print("enter_context called, frame->subName = %s\n");
    return INTERP_OK;
}

int Interp::leave_context(setup_pointer settings, bool restore)
{
    context_pointer leaving_frame = &settings->sub_context[settings->call_level];

    if (settings->call_level < 1)
    {
        ERS(INTERP_ERR_INTERNAL, NCE_CALL_STACK_UNDERRUN);
    }
    logOword("leave_context cl=%d->%d  type=%s state=%s",
             settings->call_level,
             settings->call_level - 1,
             call_typenames[leaving_frame->call_type],
             call_statenames[settings->call_state]);

    free_named_parameters(leaving_frame);
    leaving_frame->subName = NULL;
    settings->call_level--;  // drop back

    if (restore && ((leaving_frame->context_status & (CONTEXT_RESTORE_ON_RETURN | CONTEXT_VALID)) == (CONTEXT_RESTORE_ON_RETURN | CONTEXT_VALID)))
    {
        // a valid previous context was marked by an M73 as auto-restore

        // NB: this means an M71 invalidate context will prevent an
        // auto-restore on return/endsub
        CHP(restore_settings(settings, settings->call_level + 1));
    }
    return INTERP_OK;
}

/************************************************************************/
/* convert_control_functions

Returned Value: int (INTERP_OK)
Side effects:
   Changes the flow of control.

Called by: execute

Calls: control_skip_to
       control_back_to
       control_save_offset
*/

int Interp::convert_control_functions(block_pointer block,     // pointer to a block of RS274/NGC instructions
                                      setup_pointer settings)  // pointer to machine settings
{
    int status = INTERP_OK;
    context_pointer current_frame;
    offset_pointer op = NULL;

    logOword("convert_control_functions %s", o_ops[block->o_type]);

    // must skip if skipping
    if (settings->skipping_o && (0 != strcmp(settings->skipping_o, block->o_name)))
    {
        logOword("skipping to line: |%s|", settings->skipping_o);
        return INTERP_OK;
    }
    if (settings->skipping_to_sub && (block->o_type != O_sub))
    {
        logOword("skipping to sub: |%s|", settings->skipping_to_sub);
        return INTERP_OK;
    }

    // if skipping_o was set, we are now on a line which contains that O-word.
    // if skipping_to_sub was set, we are now on the 'O-name sub' definition line.

    switch (block->o_type)
    {
    case O_none:
        // not an error because we use this to signal that we
        // are not evaluating functions
        break;

    case O_sub:
        // if the level is not zero, this is a call
        // not the definition
        // if we were skipping, no longer
        if (settings->skipping_o)
        {
            logOword("sub(o_|%s|) was skipping to here", settings->skipping_o);
            // skipping to a sub means that we must define this now
            CHP(control_save_offset(block, settings));
            logOword("no longer skipping to:|%s|", settings->skipping_o);
            settings->skipping_o = NULL;  // this IS our block number
        }
        settings->skipping_to_sub = NULL;  // this IS our block number

        if (settings->call_level != 0)
        {
            logOword("call:%f:%f:%f", settings->parameters[1], settings->parameters[2], settings->parameters[3]);
        }
        else
        {
            // a definition. We're on the O<name> sub line.
            logOword("started a subroutine defn: %s", block->o_name);
            CHKS((settings->defining_sub == 1), INTERP_ERR_SYNTAX_FAILED, NCE_NESTED_SUBROUTINE_DEFN);
            CHP(control_save_offset(block, settings));

            // start skipping to the corresponding ensub.
            settings->skipping_o = block->o_name;
            settings->skipping_start = settings->sequence_number;
            settings->defining_sub = 1;
            settings->sub_name = block->o_name;
            logOword("will now skip to: |%s|", settings->sub_name);
        }
        break;

    case O_endsub:
    case O_return:

        if ((settings->call_level == 0) && (settings->sub_name == NULL))
        {
            // detect a standalone 'o<label> return|endsub'
            OERR(_("%d: not in a subroutine definition: '%s'"), settings->sequence_number, settings->linetext);
        }

        // proper label semantics (only refer to defined sub, within sub defn etc)
        // is handled in read_o() for return & endsub
        current_frame = &settings->sub_context[settings->call_level];
        CHP(execute_return(settings, current_frame, current_frame->call_type));
        break;

    case O_call:
        // only enter new frame if not reexecuting a Python handler
        // which returned INTERP_EXECUTE_FINISH
        if (settings->call_state == CS_NORMAL)
        {
            CHP(enter_context(settings, block));
        }
        current_frame = &settings->sub_context[settings->call_level];
        CHP(execute_call(settings, current_frame, current_frame->call_type));
        break;

    case O_do:
        // if we were skipping, no longer
        settings->skipping_o = NULL;
        // save the loop point
        // we hit this again on loop back -- so test first
        if (INTERP_OK != control_find_oword(block, settings, &op))  // &index))
        {
            // save offset if not found in offset table
            CHP(control_save_offset(block, settings));
        }
        break;

    case O_repeat:
        if (control_find_oword(block, settings, &op) == INTERP_OK)
        {
            if (settings->sequence_number != (op->sequence_number + 1))
                OERR(_("%d: duplicate O-word label: '%s' - defined in line %d"), settings->sequence_number, settings->linetext, op->sequence_number + 1);
        }
        else
            CHP(control_save_offset(block, settings));

        // if we were skipping, no longer
        settings->skipping_o = NULL;
        status = control_find_oword(block, settings, &op);  // &index);

        // test if not already seen OR
        // if seen and this is a repeat
        if ((status != INTERP_OK) || (op->type == block->o_type))
        {
            // this is the beginning of a 'repeat' loop
            // add it to the table if not already there
            if (status != INTERP_OK)
                CHP(control_save_offset(block, settings));

            // note the repeat count.  it should only be calculated at the
            // start of the repeat loop.
            control_find_oword(block, settings, &op);  // &index);
            if (op->repeat_count == -1)
                op->repeat_count = round_to_int(settings->test_value);

            // are we still repeating?
            if (op->repeat_count > 0)
            {
                // execute forward
                logOword("executing forward: [%s] in 'repeat' test value-- %g", block->o_name, settings->test_value);
                // one less repeat remains
                op->repeat_count--;
                // rtapi_print("op->repeat_count updated to %d\n", op->repeat_count);
            }
            else
            {
                // skip forward
                logOword("skipping forward: [%s] in 'repeat'", block->o_name);
                settings->skipping_o = block->o_name;
                settings->skipping_start = settings->sequence_number;
                // cause the repeat count to be recalculated
                // if we do this loop again
                op->repeat_count = -1;
            }
        }
        break;

    case O_while:
        if (control_find_oword(block, settings, &op) == INTERP_OK)
        {
            if ((op->type != O_do) && (settings->sequence_number != (op->sequence_number + 1)))
                OERR(_("%d: duplicate O-word label: '%s' - defined in line %d"), settings->sequence_number, settings->linetext, op->sequence_number + 1);
        }
        else
            // record only if this is a while/endwhile loop
            CHP(control_save_offset(block, settings));

        // if we were skipping, no longer
        settings->skipping_o = NULL;
        status = control_find_oword(block, settings, &op);  //  &index);

        // test if not already seen OR
        // if seen and this is a while (alternative is that it is a do)
        if ((status != INTERP_OK) || (op->type == block->o_type))
        {
            // this is the beginning of a 'while' loop
            // add it to the table if not already there
            if (status != INTERP_OK)
                CHP(control_save_offset(block, settings));

            // test the condition
            if (settings->test_value != 0.0)  // true - execute forward
                logOword("executing forward: [%s] in 'while'", block->o_name);
            else
            {
                if (control_find_whileend(block, settings, &op) == INTERP_OK)
                {
                    int old_seq_num = settings->sequence_number;
                    CHP(control_jump_to(block, settings, op));
                    settings->skipping_o = block->o_name;
                    // rtapi_print("jump to end from while sentense for false condition: %d to %d, op.sequence_num=%d\n", old_seq_num, settings->sequence_number,op->sequence_number);
                }
                else
                {
                    // logOword("skipping forward: [%s] in 'while'", block->o_name);
                    // settings->skipping_o = block->o_name;
                    // settings->skipping_start = settings->sequence_number;
                    ERS(INTERP_ERR_SYNTAX_FAILED,
                        "mismatched while-end for  exit whlie loop. ");  //continue 指令会在没有while-end的情况下回到whlie起始位置、循环条件不满足，再跳到while-end，这时报语法错误；如果while循环体中没有continue，会一直将while后面的语句当做循环体的一部分，不会报错 < TODO >
                }
            }
        }
        else
        {
            // this is the end of a 'do'
            // test the condition
            if ((settings->test_value != 0.0) && !settings->doing_break)
            {
                // true - loop on back
                logOword("looping back to: [%s] in 'do while'", block->o_name);
                CHP(control_back_to(block, settings));
            }
            else
            {
                // false
                logOword("not looping back to: [%s] in 'do while'", block->o_name);
                settings->doing_break = 0;
            }
        }
        break;

    case O_if:
        if (control_find_curroword(block, settings, &op) == INTERP_OK)
        {
            if (settings->sequence_number != (op->sequence_number + 1))
                OERR(
                    _("%d: duplicate O-word label - already defined in line %d: '%s'"), settings->sequence_number, op->sequence_number + 1, settings->linetext);
        }
        else
            CHP(control_save_offset(block, settings));

        if (settings->test_value != 0.0)
        {
            //true
            logOword("executing forward: [%s] in 'if'", block->o_name);
            settings->skipping_o = NULL;
            settings->executed_if = 1;
        }
        else
        {
            //false
            logOword("skipping forward: [%s] in 'if'", block->o_name);
            if (control_find_ifnext(block, settings, &op) == INTERP_OK)
            {
                // rtapi_print("jump to next from if sentense: %d to %d\n", settings->sequence_number, op->sequence_number);
                CHP(control_jump_to(block, settings, op));
            }
            else
            {
                settings->skipping_o = block->o_name;
                settings->skipping_start = settings->sequence_number;
            }
            settings->executed_if = 0;
        }
        break;

    case O_elif:
        if (control_find_ifhead(block, settings, &op) != INTERP_OK)
            OERR(_("%d: undefined O-word label 1: '%s'"), settings->sequence_number, settings->linetext);
        if (op && op->type != O_if)
            OERR(_("%d: no matching 'if' label: '%s' (found '%s' in line %d)"),
                 settings->sequence_number,
                 settings->linetext,
                 o_ops[op->type] + 2,
                 op->sequence_number + 1);

        if (control_find_curroword(block, settings, &op) != INTERP_OK)
        {
            CHP(control_save_offset(block, settings));
        }

        if ((settings->skipping_o) && (0 != strcmp(settings->skipping_o, block->o_name)))
        {
            //!!!KL -- the if conditions here say that we were skipping
            //!!!KL but that the target o_name is not ours.
            //!!!KL so we should continue skipping -- that's not what
            //!!!KL the code below says.

            // mah: so?
#if 0
	    // we were not skipping -- start skipping
	    logOword("start skipping forward: [%s] in 'elseif'",
		     block->o_name);
	    settings->skipping_o = block->o_name;
	    settings->skipping_start = settings->sequence_number;
	    return INTERP_OK;
#else
            // we were skipping -- continue skipping
            logOword("continue skipping forward: [%s] in 'elseif'", block->o_name);
            return INTERP_OK;
#endif
        }

        // we were skipping
        // but were we ever not skipping
        if (settings->executed_if)
        {
            // we have already executed, keep on skipping
            logOword("already executed, continue skipping forward: [%s] in 'elseif'", block->o_name);

            if (control_find_ifend(block, settings, &op) == INTERP_OK)
            {
                // rtapi_print("jump to end from elseif sentense: %d to %d\n", settings->sequence_number, op->sequence_number);
                CHP(control_jump_to(block, settings, op));
                return INTERP_OK;
            }
            else
            {
                settings->skipping_o = block->o_name;
                settings->skipping_start = settings->sequence_number;
                return INTERP_OK;
            }
        }

        if (settings->test_value != 0.0)
        {
            //true -- start executing
            logOword("start executing forward: [%s] in 'elseif'", block->o_name);
            settings->skipping_o = NULL;
            settings->executed_if = 1;
        }
        else
        {
            //false
            logOword("continue skipping forward: [%s] in 'elseif'", block->o_name);
            if (control_find_ifnext(block, settings, &op) == INTERP_OK)
            {
                // rtapi_print("jump to next from elseif: %d to %d\n", settings->sequence_number, op->sequence_number);
                CHP(control_jump_to(block, settings, op));
            }
        }
        break;

    case O_else:
        if (control_find_ifhead(block, settings, &op) != INTERP_OK)
        {
            rtapi_print("failed to find if head for else sentense\n");
            OERR(_("%d: undefined O-word label 3: '%s'"), settings->sequence_number, settings->linetext);
        }
        if (op && op->type != O_if)
            OERR(_("%d: no matching 'if' label: '%s' (found '%s' in line %d)"),
                 settings->sequence_number,
                 settings->linetext,
                 o_ops[op->type] + 2,
                 op->sequence_number + 1);

        if (control_find_curroword(block, settings, &op) != INTERP_OK)
        {
            CHP(control_save_offset(block, settings));
        }

        // were we ever not skipping
        if (settings->executed_if)
        {
            // we have already executed, skip
            logOword("already executed, skipping forward: [%s] in 'else'", block->o_name);
            if (control_find_ifend(block, settings, &op) == INTERP_OK)
            {
                // rtapi_print("jump to end from else sentense: %d to %d\n", settings->sequence_number, op->sequence_number);
                CHP(control_jump_to(block, settings, op));
                return INTERP_OK;
            }
            else
            {
                settings->skipping_o = block->o_name;
                settings->skipping_start = settings->sequence_number;
                return INTERP_OK;
            }
        }

        if ((settings->skipping_o) && (0 == strcmp(settings->skipping_o, block->o_name)))
        {
            // we were skipping so stop skipping
            logOword("stop skipping forward: [%s] in 'else'", block->o_name);
            settings->executed_if = 1;
            settings->skipping_o = NULL;
        }
        else
        {
            // we were not skipping -- so skip
            logOword("start skipping forward: [%s] in 'else'", block->o_name);
        }
        break;

    case O_endif:
        if (control_find_ifhead(block, settings, &op) != INTERP_OK)
            OERR(_("%d: undefined O-word label 5: '%s'"), settings->sequence_number, settings->linetext);
        if (op && op->type != O_if)
            OERR(_("%d: no matching label if: '%s' (found '%s' in line %d): '%s'"),
                 settings->sequence_number,
                 block->o_name,
                 o_ops[op->type] + 2,
                 op->sequence_number + 1,
                 settings->linetext);

        // rtapi_print("end if +++++++++o_type = %d, op->type = %d\n", block->o_type, op->type);
        if (control_find_curroword(block, settings, &op) != INTERP_OK)
        {
            CHP(control_save_offset(block, settings));
        }

        // stop skipping if we were
        settings->skipping_o = NULL;
        logOword("stop skipping forward: [%s] in 'endif'", block->o_name);
        // the KEY -- outside if clearly must have executed
        // or this would not have executed
        settings->executed_if = 1;
        break;

    case O_break:
        if (control_find_oword(block, settings, &op) != INTERP_OK)
            OERR(_("%d: undefined O-word label 7: '%s'"), settings->sequence_number, settings->linetext);
        if (op && (op->type != O_while) && (op->type != O_do))
            OERR(_("%d: no matching while/do label: '%s' (found '%s' in line %d)"),
                 settings->sequence_number,
                 settings->linetext,
                 o_ops[op->type] + 2,
                 op->sequence_number + 1);

        if (control_find_whileend(block, settings, &op) == INTERP_OK)
        {
            int old_seq_num = settings->sequence_number;
            CHP(control_jump_to(block, settings, op));
            // settings->sequence_number++;  // skip the end
            rtapi_print("jump to end from while sentense for break: %d to %d\n", old_seq_num, settings->sequence_number);
            while (m_owordIdMap.size())
            {
                std::pair<int, std::string> lastItem = m_owordIdMap.back();
                std::string strBlkName(block->o_name);
                settings->skipping_o = block->o_name;
                strBlkName = strBlkName.substr(strBlkName.find_first_of("#") + 1);
                if (strcmp(lastItem.second.c_str(), strBlkName.c_str()) == 0 && (lastItem.first == O_while || lastItem.first == O_do))
                {
                    break;
                }
                rtapi_print("pop back: %s, %s, %d\n", lastItem.second.c_str(), strBlkName.c_str(), lastItem.first);
                m_owordIdMap.pop_back();
            }
            rtapi_print("settings:  %d,%d\n", settings->sequence_number, settings->skipping_start);
            return INTERP_OK;
        }
        else
        {
            // // start skipping
            // settings->skipping_o = block->o_name;
            // settings->skipping_start = settings->sequence_number;
            // settings->doing_break = 1;
            ERS(INTERP_ERR_SYNTAX_FAILED, "mismatched while-end for break. ");  // break 指令没有找到对应的 while-end；报语法错误
        }

        logOword("start skipping forward: [%s] in 'break'", block->o_name);
        break;

    case O_continue:
        rtapi_print("contrtl_find_owrorkd called: %s, %d\n", __FILE__, __LINE__);
        if (control_find_oword(block, settings, &op) != INTERP_OK)
            OERR(_("%d: undefined O-word label 8: '%s'"), settings->sequence_number, settings->linetext);
        if (op && (op->type != O_while) && (op->type != O_do))
            OERR(_("%d: no matching while/do label: '%s' (found '%s' in line %d)"),
                 settings->sequence_number,
                 settings->linetext,
                 o_ops[op->type] + 2,
                 op->sequence_number + 1);

        // if already skipping, do nothing
        if ((settings->skipping_o) && (0 == strcmp(settings->skipping_o, block->o_name)))
        {
            logOword("already skipping: [%s] in 'continue'", block->o_name);
            return INTERP_OK;
        }
        if (control_find_oword(block, settings, &op) == INTERP_OK)
        {
            rtapi_print("jump to end from while sentense for continue: %d to %d\n", settings->sequence_number, op->sequence_number);
            CHP(control_jump_to(block, settings, op));
            // m_owordIdMap need to be updated
            while (m_owordIdMap.size())
            {
                std::pair<int, std::string> lastItem = m_owordIdMap.back();
                std::string strBlkName(block->o_name);
                strBlkName = strBlkName.substr(strBlkName.find_first_of("#") + 1);
                m_owordIdMap.pop_back();
                rtapi_print("pop back: %s, %s, %d\n", lastItem.second.c_str(), strBlkName.c_str(), lastItem.first);
                if (strcmp(lastItem.second.c_str(), strBlkName.c_str()) == 0 && (lastItem.first == O_while || lastItem.first == O_do))
                {
                    break;
                }
            }
            return INTERP_OK;
        }
        else
        {
            // start skipping
            settings->skipping_o = block->o_name;
            settings->skipping_start = settings->sequence_number;
            settings->doing_continue = 1;
            logOword("start skipping forward: [%s] in 'continue'", block->o_name);
        }
        break;

    case O_endrepeat:
    case O_endwhile:
        if (control_find_oword(block, settings, &op) != INTERP_OK)
        {
            OERR(_("%d: undefined O-word label 9: '%s'"), settings->sequence_number, settings->linetext);
        }
        else if (block->o_type == O_endwhile)
        {
            if (control_find_curroword(block, settings, &op) != INTERP_OK)
            {
                CHP(control_save_offset(block, settings));
            }
        }

        // rtapi_print("end while +++++++++o_type = %d, op->type = %d\n", block->o_type, op->type);
        if (((block->o_type == O_endrepeat) && (op->type != O_repeat)))
            OERR(_("%d: no matching label: '%s' (found '%s' in line %d)"),
                 settings->sequence_number,
                 settings->linetext,
                 o_ops[op->type] + 2,
                 op->sequence_number + 1);

        // end of a while loop
        logOword("endwhile: skipping_o:%s", settings->skipping_o);
        if ((settings->skipping_o) && (0 == strcmp(settings->skipping_o, block->o_name)))
        {
            // we were skipping, so this is the end
            settings->skipping_o = NULL;

            if (settings->doing_continue)
            {
                settings->doing_continue = 0;

                // loop on back
                logOword("looping back (continue) to: [%s] in while/repeat", block->o_name);
                CHP(control_back_to(block, settings));
            }
            else
            {
                // not doing continue, we are done
                logOword("falling thru the complete while/repeat: [%s]", block->o_name);
                return INTERP_OK;
            }
        }
        else
        {
            // loop on back
            logOword("looping back to: [%s] in 'endwhile/endrepeat'", block->o_name);
            CHP(control_back_to(block, settings));
        }
        break;

    default:
        // FIXME !!!KL should probably be an error
        return INTERP_ERROR;
        break;
    }
    // return status;
    return INTERP_OK;
}
//========================================================================
// End of functions for control stuff (O-words)
//========================================================================
